#include "PID.h"

#include <stdlib.h>
#define MAX_I_SUM 1000        // 积分项最大累积值
#define PID_DEADBAND 1        // 死区范围（误差小于此值认为为0）
PID_Struct Motor_PID_left;
PID_Struct Motor_PID_Right;

void Motor_Angle_PID_Init_Left(void)
{
	Motor_PID_left.Kp = 32.5;			//����ϵ��
	Motor_PID_left.Ti = 1.5;		//����ϵ��
	Motor_PID_left.Td = 0.7;			//΢��ϵ��
	
	Motor_PID_left.SetValue =  80;	//�û��趨ֵ
	Motor_PID_left.T = 3; 			//PID��������Ϊ20����λ20ms
	
	Motor_PID_left.PWMCycle = 7200;	//PWM����Ϊ10ms
	Motor_PID_left.Out0 = 1;			//PID�������
}

void Motor_Angle_PID_Cal_Left(void)
{
	float Ki, Kd, DletaK;

	if (Motor_PID_left.PIDCounter < Motor_PID_left.T) return;

	// --- 计算误差 ---
	Motor_PID_left.EK = Motor_PID_left.SetValue - Motor_PID_left.GetValue;

	// --- 死区 ---
	if (abs(Motor_PID_left.EK) < PID_DEADBAND)
		Motor_PID_left.EK = 0;

	// --- 比例项 ---
	Motor_PID_left.Pout = Motor_PID_left.EK * Motor_PID_left.Kp;

	// --- 积分项 ---
	Motor_PID_left.SigmaEK += Motor_PID_left.EK;

	// 限制积分项累加大小
	if (Motor_PID_left.SigmaEK > MAX_I_SUM)
		Motor_PID_left.SigmaEK = MAX_I_SUM;
	else if (Motor_PID_left.SigmaEK < -MAX_I_SUM)
		Motor_PID_left.SigmaEK = -MAX_I_SUM;

	Ki = Motor_PID_left.Kp * Motor_PID_left.T / Motor_PID_left.Ti;
	Motor_PID_left.Iout = Ki * Motor_PID_left.SigmaEK;

	// --- 微分项 ---
	DletaK = Motor_PID_left.EK - Motor_PID_left.EK_1;
	Kd = Motor_PID_left.Kp * Motor_PID_left.Td / Motor_PID_left.T;
	Motor_PID_left.Dout = DletaK * Kd;

	// --- PID 输出汇总 ---
	Motor_PID_left.Out = Motor_PID_left.Pout + Motor_PID_left.Iout + Motor_PID_left.Dout;

	// --- 限制输出 ---
	if (Motor_PID_left.Out > Motor_PID_left.PWMCycle)
		Motor_PID_left.Out = Motor_PID_left.PWMCycle;
	else if (Motor_PID_left.Out < Motor_PID_left.Out0)
		Motor_PID_left.Out = Motor_PID_left.Out0 = 1;

	// --- 保存当前误差、清零计数器 ---
	Motor_PID_left.EK_1 = Motor_PID_left.EK;
	Motor_PID_left.PIDCounter = 0;
}
// void Motor_Angle_PID_Cal_Left(void)
// {
// 	float KP = 0, DletaK = 0;
// 	if(Motor_PID_left.PIDCounter < Motor_PID_left.T) return;
// 	Motor_PID_left.EK = Motor_PID_left.SetValue - Motor_PID_left.GetValue;  //���㵱��ƫ��
// 	Motor_PID_left.Pout = Motor_PID_left.EK * Motor_PID_left.Kp;  //������������
//
// 	Motor_PID_left.SigmaEK += Motor_PID_left.EK; 				//����ƫ���ܺ�
// 	KP = Motor_PID_left.Kp * Motor_PID_left.T / Motor_PID_left.Ti;
// 	Motor_PID_left.Iout = KP * Motor_PID_left.SigmaEK; 		//������������
//
// 	DletaK = Motor_PID_left.EK - Motor_PID_left.EK_1;
// 	Motor_PID_left.Dout = DletaK * Motor_PID_left.Kp * Motor_PID_left.Td / Motor_PID_left.T;
//
// 	Motor_PID_left.Out = Motor_PID_left.Pout + Motor_PID_left.Iout + Motor_PID_left.Dout;
//
// 	Motor_PID_left.EK_1 = Motor_PID_left.EK; 				//����ƫ��
//
// 	if(Motor_PID_left.Out > Motor_PID_left.PWMCycle)
// 	{
// 		Motor_PID_left.Out = Motor_PID_left.PWMCycle;
// 	}
// 	else if(Motor_PID_left.Out < 0)
// 		Motor_PID_left.Out = Motor_PID_left.Out0;
// 	Motor_PID_left.PIDCounter = 0; 			//PID�������������
// }


void Motor_Angle_PID_Init_Right(void)
{
	Motor_PID_Right.Kp = 32.5;			//����ϵ��
	Motor_PID_Right.Ti = 1.5;		//����ϵ��
	Motor_PID_Right.Td = 0.7;			//΢��ϵ��
	
	Motor_PID_Right.SetValue =  80;	//�û��趨ֵ
	Motor_PID_Right.T = 3; 			//PID��������Ϊ20����λ10ms
	
	Motor_PID_Right.PWMCycle = 7200;	//PWM����Ϊ10ms
	Motor_PID_Right.Out0 = 1;			//PID�������
}	

void Motor_Angle_PID_Cal_Right(void)
{
	float Ki, Kd, DletaK;

	if (Motor_PID_Right.PIDCounter < Motor_PID_Right.T) return;

	// --- 计算误差 ---
	Motor_PID_Right.EK = Motor_PID_Right.SetValue - Motor_PID_Right.GetValue;

	// --- 死区 ---
	if (abs(Motor_PID_Right.EK) < PID_DEADBAND)
		Motor_PID_Right.EK = 0;

	// --- 比例项 ---
	Motor_PID_Right.Pout = Motor_PID_Right.EK * Motor_PID_Right.Kp;

	// --- 积分项 ---
	Motor_PID_Right.SigmaEK += Motor_PID_Right.EK;

	// 限制积分项累加大小
	if (Motor_PID_Right.SigmaEK > MAX_I_SUM)
		Motor_PID_Right.SigmaEK = MAX_I_SUM;
	else if (Motor_PID_Right.SigmaEK < -MAX_I_SUM)
		Motor_PID_Right.SigmaEK = -MAX_I_SUM;

	Ki = Motor_PID_Right.Kp * Motor_PID_Right.T / Motor_PID_Right.Ti;
	Motor_PID_Right.Iout = Ki * Motor_PID_Right.SigmaEK;

	// --- 微分项 ---
	DletaK = Motor_PID_Right.EK - Motor_PID_Right.EK_1;
	Kd = Motor_PID_Right.Kp * Motor_PID_Right.Td / Motor_PID_Right.T;
	Motor_PID_Right.Dout = DletaK * Kd;

	// --- PID 输出汇总 ---
	Motor_PID_Right.Out = Motor_PID_Right.Pout + Motor_PID_Right.Iout + Motor_PID_Right.Dout;

	// --- 限制输出 ---
	if (Motor_PID_Right.Out > Motor_PID_Right.PWMCycle)
		Motor_PID_Right.Out = Motor_PID_Right.PWMCycle;
	else if (Motor_PID_Right.Out < Motor_PID_Right.Out0)
		Motor_PID_Right.Out = Motor_PID_Right.Out0 = 1;

	// --- 保存当前误差、清零计数器 ---
	Motor_PID_Right.EK_1 = Motor_PID_Right.EK;
	Motor_PID_Right.PIDCounter = 0;
}

// void Motor_Angle_PID_Cal_Right(void)
// {
// 	float KP = 0, DletaK = 0;
// 	if(Motor_PID_Right.PIDCounter < Motor_PID_Right.T) return;
// 	Motor_PID_Right.EK = Motor_PID_Right.SetValue - Motor_PID_Right.GetValue;  //���㵱��ƫ��
// 	Motor_PID_Right.Pout = Motor_PID_Right.EK * Motor_PID_Right.Kp;  //������������
//
// 	Motor_PID_Right.SigmaEK += Motor_PID_Right.EK; 				//����ƫ�����ܺ�
// 	KP = Motor_PID_Right.Kp * Motor_PID_Right.T / Motor_PID_Right.Ti;
// 	Motor_PID_Right.Iout = KP * Motor_PID_Right.SigmaEK; 		//������������
//
// 	DletaK = Motor_PID_Right.EK - Motor_PID_Right.EK_1;
// 	Motor_PID_Right.Dout = DletaK * Motor_PID_Right.Kp * Motor_PID_Right.Td / Motor_PID_Right.T;
//
// 	Motor_PID_Right.Out = Motor_PID_Right.Pout + Motor_PID_Right.Iout + Motor_PID_Right.Dout;
//
// 	Motor_PID_Right.EK_1 = Motor_PID_Right.EK; 				//����ƫ��
//
// 	if(Motor_PID_Right.Out > Motor_PID_Right.PWMCycle)
// 	{
// 		Motor_PID_Right.Out = Motor_PID_Right.PWMCycle;
// 	}
// 	else if(Motor_PID_Right.Out < 0)
// 		Motor_PID_Right.Out = Motor_PID_Right.Out0;
// 	Motor_PID_Right.PIDCounter = 0; 			//PID�������������
// }
